#ifndef __ITKEXT__IMAGE__MOMENTSCALCULATOR__TXX__
#define __ITKEXT__IMAGE__MOMENTSCALCULATOR__TXX__

#include <vnl/algo/vnl_real_eigensystem.h>
#include <vnl/algo/vnl_symmetric_eigensystem.h>

namespace itkExt
{
  // -----------------------------------------------------------------------
  template< class S, unsigned int D >
  MomentsCalculator< S, D >::
  MomentsCalculator( )
  {
    this->Reset( );
  }

  // -----------------------------------------------------------------------
  template< class S, unsigned int D >
  void MomentsCalculator< S, D >::
  Reset( )
  {
    this->m_Updated = false;
    this->m_Mass = TScalar( 0 );
    this->m_As.Fill( TScalar( 0 ) );
    this->m_Gs.Fill( TScalar( 0 ) );
  }

  // -----------------------------------------------------------------------
  template< class S, unsigned int D >
  void MomentsCalculator< S, D >::
  AddMass( const TPoint& pnt, const TScalar& mass )
  {
    this->AddMass( pnt.GetVectorFromOrigin( ), mass );
  }

  // -----------------------------------------------------------------------
  template< class S, unsigned int D >
  void MomentsCalculator< S, D >::
  AddMass( const TVector& vec, const TScalar& mass )
  {
    // Iteration actualization
    this->m_Mass += mass;
    for( unsigned int i = 0; i < D; i++ )
    {
      this->m_As[ i ] += vec[ i ] * mass;
      for( unsigned int j = 0; j < D; j++ )
        this->m_Gs[ i ][ j ] += vec[ i ] * vec[ j ] * mass;

    } // rof

    this->m_Updated = false;
  }

  // -----------------------------------------------------------------------
  template< class S, unsigned int D >
  void MomentsCalculator< S, D >::
  Compute( )
  {
    if( this->m_Updated )
      return;

    // Init values
    this->m_Updated = true;

    // End if no mass
    if( this->m_Mass == TScalar( 0 ) )
      return;

    for( unsigned int i = 0; i < D; i++ )
    {
      this->m_Center[ i ] = this->m_As[ i ] / this->m_Mass;
      for( unsigned int j = 0; j < D; j++ )
      {
        TScalar g = this->m_Gs[ i ][ j ];
        TScalar a = this->m_As[ i ];
        TScalar b = this->m_As[ j ];
        this->m_Inertia[ i ][ j ] = ( ( a * b ) / this->m_Mass ) - g;
        if( i == j )
          this->m_Inertia[ i ][ j ] *= TScalar( -1 );

      } // rof

    } // rof

    // Compute principal moments and axes
    vnl_symmetric_eigensystem< TScalar > e( this->m_Inertia.GetVnlMatrix( ) );
    for( unsigned int i = 0; i < D; i++ )
      this->m_PValues[ i ] = e.D( i, i );
    TMatrix pm = e.V.transpose( );

    // Add a final reflection if needed for a proper rotation,
    // by multiplying the last row by the determinant
    vnl_real_eigensystem eRot( pm.GetVnlMatrix( ) );
    vcl_complex< TScalar > det( TScalar( 1 ), TScalar( 0 ) );
    for( unsigned int i = 0; i < D; i++ )
      det *= eRot.D( i, i );
    for( unsigned int i = 0; i < D; i++ )
      pm[ D - 1 ][ i ] *= std::real( det );

    this->m_PMatrix = pm;
  }

  // -----------------------------------------------------------------------
  template< class S, unsigned int D >
  void MomentsCalculator< S, D >::
  AlignProperMatrix( const TVector& dir )
  {
    if( !this->m_Updated )
      this->Compute( );

    TVector x;
    x.Fill( 0 );
    x[ 0 ] = 1;

    if( ( ( this->m_PMatrix * x ) * dir ) < TScalar( 0 ) )
    {
      for( unsigned int i = 0; i < D; i++ )
      {
        this->m_PMatrix[ i ][ 0 ] *= TScalar( -1 );
        this->m_PMatrix[ i ][ 2 ] *= TScalar( -1 );

      } // rof

    } // fi
  }

  // -----------------------------------------------------------------------
  template< class S, unsigned int D >
  void MomentsCalculator< S, D >::
  AlignProperMatrix( const TMatrix& rot )
  {
    TVector x;
    x.Fill( 0 );
    x[ 0 ] = TScalar( 1 );
    this->AlignProperMatrix( rot * x );
  }

  // -----------------------------------------------------------------------
  template< class S, unsigned int D >
  typename MomentsCalculator< S, D >::
  THMatrix MomentsCalculator< S, D >::
  GetHomogeneousMatrix( ) const
  {
    THMatrix h;
    h.SetIdentity( );

    for( unsigned int x = 0; x < D; x++ )
    {
      h[ x ][ D ] = this->m_Center[ x ];
      for( unsigned int y = 0; y < D; y++ )
        h[ x ][ y ] = this->m_PMatrix[ x ][ y ];

    } // rof

    return( h );
  }

} // ecapseman

#endif // __ITKEXT__IMAGE__MOMENTSCALCULATOR__TXX__

// eof - $RCSfile: itkExtMomentsCalculator.txx,v $
